package meshGrammar;

import java.awt.geom.Line2D;



public class Vec3D {
public float x,y,z;

public Vec3D(float x,float y,float z){
	this.x=x;
	this.y=y;
	this.z=z;
}
public Vec3D(double x,double y,double z){
	this.x=(float)x;
	this.y=(float)y;
	this.z=(float)z;
}
public Vec3D(float x,float y){
	this.x=x;
	this.y=y;
}

public Vec3D(){
}


public boolean equalPosition3D(Vec3D v,float dist){
	if (this.dist(v)<dist)return true;
	return false;
}
public boolean equalPosition(Vec3D v){
	if (v.x==x&&v.y==y&&v.z==z)return true;
	return false;
}

public void setPosition(float x,float y,float z){
	this.x=x;
	this.y=y;
	this.z=z;
}

public Vec3D getCopy(){
	return new Vec3D(x,y,z);
}

public void add(Vec3D v){
	this.x+=v.getX();
	this.y+=v.getY();
	this.z+=v.getZ();
}

public void sub(Vec3D v){
	this.x-=v.getX();
	this.y-=v.getY();
	this.z-=v.getZ();
}

public float mag(){
	return (float)Math.sqrt(magSqr());
}

public float magSqr(){
	return x*x+y*y+z*z;
}

public void mult(float f){
	this.x*=f;
	this.y*=f;
	this.z*=f;
}

public void div(float f){
	this.x/=f;
	this.y/=f;
	this.z/=f;
}

public float distSqr(float x,float y,float z){
	float dX=this.x-x;
	float dY=this.y-y;
	float dZ=this.z-z;
	return dX*dX+dY*dY+dZ*dZ;
}

public float distSqr(Vec3D v){
	return distSqr(v.getX(),v.getY(),v.getZ());
}

public float dist(float x,float y,float z){
	return (float)Math.sqrt(distSqr(x,y,z));
}



public float dist(Vec3D v){
	return (float)Math.sqrt(distSqr(v));
}
public float dot(Vec3D b) {
	return x * b.getX() + y * b.getY() + z * b.getZ();
}
public float dot(float cX,float cY,float cZ) {
	return x * cX + y * cY + z * cZ;
}
public Vec3D cross(Vec3D b) {
	float cX = y * b.getZ() - z * b.getY();
	float cY = z * b.getX() - x * b.getZ();
	float cZ = x * b.getY() - y * b.getX();
	return new Vec3D(cX,cY,cZ);
}
public void normalize(){
	div(mag());
}
public void invert(){
	mult(-1);
}

public void setLength(float l) {
	// TODO Auto-generated method stub
	this.mult(l/mag());
}
@Override
public String toString() {
	// TODO Auto-generated method stub
	return super.toString()+" x: "+x+" y: "+y+" z: "+z;
}

public float getAngleY(){
	float l2D=(float)Math.sqrt(x*x+y*y);
	return (float)Math.atan2(l2D, z);
}

public float getAngleZZ(){
	return (float)Math.atan2(y, x);
}

	float getYAngle(float x1, float y1, float z1, float x2, float y2, float z2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		float dZ = z2 - z1;
		float dXY = (float) Math.sqrt(dX * dX + dY * dY);
		return (float) Math.atan2(dZ, -dXY);
	}

	float getZAngle(float x1, float y1, float x2, float y2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		return (float) Math.atan2(dY, dX);
	}

public float getZ() {
	return z;
}

public Vec3D getOrthoVector(){
	// one of endless;
	Vec3D v=null;
	if (z!=0) v=new Vec3D(1,1,-(x+y)/z);
	else if (y!=0) v=new Vec3D(1,-(x+z)/y,1);
	else if (x!=0) v=new Vec3D(-(y+z)/x,1,1);
	v.normalize();
	return v;
}





public void setZ(float z) {
	this.z = z;
}

public static Vec3D cross(Vec3D a, Vec3D b) {
	float x = a.getY() * b.getZ() - a.getZ() * b.getY();
	float y = a.getZ() * b.getX() - a.getX() * b.getZ();
	float z = a.getX() * b.getY() - a.getY() * b.getX();
	return new Vec3D(x, y, z);
}
public static float dot(Vec3D a,Vec3D b) {
	return a.getX()* b.getX() + a.getY() * b.getY() + a.getZ() * b.getZ();
}
public static float mag(Vec3D v){
	return (float)Math.sqrt(magSqr(v));
}
public static float magSqr(Vec3D v){
	return  v.getX()* v.getX()+ v.getY()*v.getY()+v.getZ()*v.getZ();
}
public static Vec3D div(Vec3D p, float f){
	return new Vec3D(p.getX()/f,p.getY()/f,p.getZ()/f);
}
public static float distSqr(Vec3D v1,Vec3D v2){
	float dX=v1.getX()-v2.getX();
	float dY=v1.getY()-v2.getY();
	float dZ=v1.getZ()-v2.getZ();
	
	return dX*dX+dY*dY+dZ*dZ;
}
public static float dist(float x1,float y1,float z1,float x2,float y2,float z2){
	return (float)Math.sqrt(distSqr(x1,y1,z1,x2,y2,z2));
}
public static float distSqr(float x1,float y1,float z1,float x2,float y2,float z2){
	float dX=x2-x1;
	float dY=y2-y1;
	float dZ=z2-z1;
	
	return dX*dX+dY*dY+dZ*dZ;
}
public static float dist(Vec3D v1,Vec3D v2){
	float d=distSqr(v1,v2);
	return (float)Math.sqrt(d);
}

public static Vec3D normalize(Vec3D p){
	return  div(p,mag(p));
}
public static Vec3D getNormal(Vec3D a,Vec3D b,Vec3D c){
	Vec3D ab=Vec3D.sub(b,a);
	Vec3D ac=Vec3D.sub(c,a);
	Vec3D cross=Vec3D.cross(ac,ab);
	cross.normalize();
	return cross;
}




public static Vec3D rotateAroundLine(Vec3D p,Vec3D o,Vec3D vec,float angle){
	//http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/
	//If we multiply this times ⟨x,y,z⟩ we can obtain a function of of ten variables that yields the result of 
	//rotating the point (x,y,z) about the line through (a,b,c) with direction vector ⟨u,v,w⟩ (where u2 + v2 + w2 = 1) by the angle θ.
	float x=p.getX();
	float y=p.getY();
	float z=p.getZ();
	float a=o.getX();
	float b=o.getY();
	float c=o.getZ();
	float u=vec.getX();
	float v=vec.getY();
	float w=vec.getZ();
	float cos=(float)Math.cos(angle);
	float sin=(float)Math.sin(angle);
	float newX=(a*(v*v+w*w)-u*(b*v+c*w-u*x-v*y-w*z))*(1-cos)+x*cos+(-c*v+b*w-w*y+v*z)*sin;
	float newY=(b*(u*u+w*w)-v*(a*u-c*w-u*x-v*y-w*z))*(1-cos)+y*cos+(c*u-a*w+w*x-u*z)*sin;
	float newZ=(c*(u*u+v*v)-w*(a*u+b*v-u*x-v*y-w*z))*(1-cos)+z*cos+(-b*u+a*v-v*x+u*y)*sin;
	return new Vec3D(newX,newY,newZ);
}

public static Vec3D mult(Vec3D p1,float m){
	
	return new Vec3D(p1.getX()*m,p1.getY()*m,p1.getZ()*m);
}
public static Vec3D add(Vec3D v1,Vec3D v2){
	
	return new Vec3D(v1.getX()+v2.getX(),v1.getY()+v2.getY(),v1.getZ()+v2.getZ());
}
public static Vec3D sub(Vec3D v1,Vec3D v2){
	
	return new Vec3D(v1.getX()-v2.getX(),v1.getY()-v2.getY(),v1.getZ()-v2.getZ());
}


public static Vec3D getBetween(Vec3D v1,Vec3D v2,float f){
	float cX=f*(v2.getX()-v1.getX())+v1.getX();
	float cY=f*(v2.getY()-v1.getY())+v1.getY();
	float cZ=f*(v2.getZ()-v1.getZ())+v1.getZ();
	return new Vec3D(cX,cY,cZ);
}
public float getX() {
	return x;
}
public void setX(float x) {
	this.x = x;
}
public float getY() {
	return y;
}
public void setY(float y) {
	this.y = y;
}

final static float SMALL_NUM=0.0000001f;


public static Vec3D getNormalUnscaled(Vec3D p1, Vec3D p2, Vec3D p3) {
	Vec3D v1 = Vec3D.sub(p2, p1);
	Vec3D v2 = Vec3D.sub(p3, p1);
	Vec3D norm=v1.cross(v2);
	return norm;
}


public static float getTriangleArea(Vec3D a, Vec3D b,Vec3D c){
	return getTriangleArea(a.getX(),a.getY(),a.getZ(),b.getX(),b.getY(),b.getZ(),c.getX(),c.getY(),c.getZ());
}

public static float getTriangleArea(float xa, float ya, float za, float xb, float yb, float zb, float xc, float yc, float zc){
	return (float)(0.5f*Math.sqrt(Math.pow(determinant(xa,xb,xc,ya,yb,yc,1,1,1),2)+Math.pow(determinant(ya,yb,yc,za,zb,zc,1,1,1),2)+Math.pow(determinant(za,zb,zc,xa,xb,xc,1,1,1),2)))
	;
}
//http://stackoverflow.com/questions/1406029/how-to-calculate-the-volume-of-a-3d-mesh-object-the-surface-of-which-is-made-up
public static float signedVolumeOfTriangle(Vec3D p1, Vec3D p2, Vec3D p3) {
    float v321 = p3.getX()*p2.getY()*p1.getZ();
    float v231 = p2.getX()*p3.getY()*p1.getZ();
    float v312 = p3.getX()*p1.getY()*p2.getZ();
    float v132 = p1.getX()*p3.getY()*p2.getZ();
    float v213 = p2.getX()*p1.getY()*p3.getZ();
    float v123 = p1.getX()*p2.getY()*p3.getZ();
    return (1.0f/6.0f)*(-v321 + v231 + v312 - v132 - v213 + v123);
}



public static float determinant(float a,float b,float c, float d,float e,float f, float g, float h, float i){
	 return (a*e*i-a*f*h-b*d*i+b*f*g+c*d*h-c*e*g);
	 }


public static Vec3D getGravityCenter(Vec3D[] points){
	Vec3D p = new Vec3D();
	for (int i = 0; i < points.length; i++) {
		Vec3D cP = points[i];
		p.add(cP);
	}
	p.mult(1f / points.length);
	return p;
}


public static float getArea2D(Vec3D[] points) {
	float area = 0;
	int j;
	for (int i = 0; i < points.length; i++) {
		j = i + 1;
		if (j ==  points.length)
			j = 0;
		Vec3D p1 = points[i];
		Vec3D p2 = points[j];
		area += (p1.getX() + p2.getX()) * (p2.getY() - p1.getY());
	}
	return area / 2f;
}

public static float getPerimeter(Vec3D[] points){
	float sum=0;
	for (int i = 0; i < points.length; i++) {
		Vec3D n1 =points[i];
		Vec3D n2 = points[(i+1)% points.length];
		sum+=Vec3D.dist(n1, n2);
	}
	return sum;
}
public static Vec3D intersect_VerRayTriangle( Vec3D p0,Vec3D p1,Vec3D p2,Vec3D r0,Vec3D r1 )
{
	 
     // triangle vectors
  
    // is I inside T?
  
   // sameNormal
    if (Line2D.relativeCCW( p0.getX(), p0.getY(),  p1.getX(), p1.getY(),p2.getX(), p2.getY())==1){
    	Vec3D t=p0;
    	p0=p1;
    	p1=t;
    }
  
    Vec3D i=rayTriIntersection(p0,p1,p2,r0,r1);
   if (i==null){
	   return null;
   }
    if (Line2D.relativeCCW( p2.getX(), p2.getY(),  p0.getX(), p0.getY(),r0.getX(), r0.getY())==1)return null;	   
   if (Line2D.relativeCCW( p0.getX(), p0.getY(),  p1.getX(), p1.getY(),r0.getX(), r0.getY())==1)return null;
  if (Line2D.relativeCCW( p1.getX(), p1.getY(),  p2.getX(), p2.getY(),r0.getX(), r0.getY())==1)return null;  
 
  return new Vec3D();
/*  Plane plane=new Plane(p2,p1,p0);
    float d=plane.getDistanceSigned(r0);
    if (d<0)return new Vec3D();
    else return null;*/
    //return i;                      // I is in T
}
public static Vec3D rayTriIntersection(Vec3D p0,Vec3D p1,Vec3D p2,Vec3D r0,Vec3D r1){
	Vec3D e1=Vec3D.sub(p1, p0);
	Vec3D e2=Vec3D.sub(p2, p0);
	Vec3D d=Vec3D.sub(r1,r0);
	Vec3D h =Vec3D.cross(d,e2);
	float a=Vec3D.dot(e1, h);
	if (a>-SMALL_NUM&&a<SMALL_NUM)return null;
	float f=1f/a;
	Vec3D s = Vec3D.sub(r0, p0);
	float u = f * Vec3D.dot(s, h);
	if (u < 0 || u > 1)
		return null;
	Vec3D q = Vec3D.cross(s, e1);
	float v = f * Vec3D.dot(d, q);
	if (v < 0 || u + v > 1)
		return null;
	float t = f * Vec3D.dot(e2, q);
	if (t > SMALL_NUM) {
		d.mult(t);
		d.add(r0);
		return d;
	}
	return null;
}

public static float getDistanceSignedToPlane( Vec3D p0,Vec3D p1,Vec3D p2,float x,float y,float z ){
	//this.p=new Vec3D(p);
	Vec3D normal= Vec3D.getNormal(p0, p1, p2);
	normal.normalize();
	float d0=-normal.dot(p0);
	return normal.dot(x, y, z)+d0;
}
public static float getDistanceSignedToPlane( Vec3D p0,Vec3D p1,Vec3D p2,Vec3D v){
	return getDistanceSignedToPlane(p0,p1,p2,v.x,v.y,v.z);
}
}
